/*****************************************************
封装小车视觉模块识别部分. 
包含视觉模块初始化、检测二维码、色块
*****************************************************/


//视觉模块初始化
void vision_init(){
  //分配内存
  wk = malloc(sizeof(WonderK210));
  //开启串口接收
  //这里不需要再调用初始化串口函数 Serial.begin(115200); 在 serial_begin() 函数里已经初始化了  
  wk->serial_begin();
  //分配内存
  result = malloc(sizeof(Find_Box_Msg_st));
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
/*
中间值  -371  -181
*/
//通过色环处的线来初步校准小车的位置
// int motor_speed_now = 15;
void calibrate_the_car_through_line(){
  // servo_move(51, 139, 160, 25, 100);//delay(1000);
  // lift_move(45);//delay(1000);
  // servo_move(115, 139, 160, 25, 100);//delay(1000);
  // servo_move(115, 80, 160, 25, 100);//delay(1000);

  
  judge_msg.calibrate_car_flag = true;
  judge_msg.isnot_find_line = true;
  judge_msg.start_judge_once = 0;
  judge_msg.input_angle_judge = true;
  judge_msg.input_position_judge = false;
  //首先进入角度调整
  while(judge_msg.calibrate_car_flag){
    for(int i=0;i<1;i++){
      judge_msg.send_commond_msg = "judge_car_start";
      Serial2.println(judge_msg.send_commond_msg);
      delay(100);

      //首先，小车进入角度调整判断------------------------------------------------
      if(judge_msg.input_angle_judge){
          for(int j=0;j<3;j++){
            wk->update_data(); //更新消息
            bool rt = wk->recive_box_msg(result , K210_FIND_APRILTAGS);//获取消息
            if(rt){
              judge_msg.x1 = result->x;
              judge_msg.y1 = result->y;
              judge_msg.x2 = result->w;
              judge_msg.y2 = result->h;
              judge_msg.line_slope = float(judge_msg.y2 - judge_msg.y1) / float(judge_msg.x2 - judge_msg.x1);
              judge_msg.line_position = (judge_msg.y2 + judge_msg.y1) / 2;

              Serial.print("slope:"); Serial.print(judge_msg.line_slope, 4);
              Serial.print(" position:"); Serial.println(judge_msg.line_position);
              float car_need_to_turn_angle = 0.0;
              if(judge_msg.line_slope < -0.038){
                if(judge_msg.line_slope <= -0.20){ judge_msg.line_slope = -0.20;}
                car_need_to_turn_angle = mapFloat(judge_msg.line_slope,  -0.20, -0.038, -20,  0);
                Serial.print("小车需要左转动角度:");Serial.println(-car_need_to_turn_angle);
                car_move(car_msg.turnleft, fabs(car_need_to_turn_angle));
              }
              else if( (judge_msg.line_slope >= -0.038) && (judge_msg.line_slope <= -0.017) ){
                Serial.println("小车不用转");
                car_pwm(car_msg.stop, 0, 0, 0, 0);
                  judge_msg.input_angle_judge = false;
                  judge_msg.input_position_judge = true;
              }
              else if(judge_msg.line_slope == 0){
                car_pwm(car_msg.stop, 0, 0, 0, 0);
                judge_msg.input_angle_judge = false;
                judge_msg.input_position_judge = true;
                Serial.println("小车不用转");
              }
              else{
                if(judge_msg.line_slope >= 0.2){ judge_msg.line_slope = 0.2;}
                car_need_to_turn_angle = mapFloat(judge_msg.line_slope,  -0.017, 0.2, 0,  15);
                Serial.print("小车需要右转动角度:");Serial.println(car_need_to_turn_angle);
                car_move(car_msg.turnright, fabs(car_need_to_turn_angle));
              }
              wk->clear_rec();
            }
            else{
              //Serial.println("nothing");
            }
            wk->clear_rec();
          }
      }

      //接着，小车进入位置调整判断------------------------------------------------
      if(judge_msg.input_position_judge){
          for(int j=0;j<3;j++){
            wk->update_data(); //更新消息
            bool rt = wk->recive_box_msg(result , K210_FIND_APRILTAGS);//获取消息
            if(rt){
              judge_msg.x1 = result->x;
              judge_msg.y1 = result->y;
              judge_msg.x2 = result->w;
              judge_msg.y2 = result->h;
              judge_msg.line_slope = float(judge_msg.y2 - judge_msg.y1) / float(judge_msg.x2 - judge_msg.x1);
              judge_msg.line_position = (judge_msg.y2 + judge_msg.y1) / 2;
              Serial.print(" position:"); Serial.println(judge_msg.line_position);
              //如果检测到的直线纵坐标小于图片像素的190位置，表示小车距离色环处的位置过远，小车需要靠近
              float data_now = 0.0;
              if(judge_msg.line_position < 190){
                data_now = mapFloat(judge_msg.line_slope,  0, 190, 0.025,  0);
                car_move(car_msg.transleft, data_now);
              }
              // else if( (judge_msg.line_position >= 190) && (judge_msg.line_position < 210)){
              else if( (judge_msg.line_position >= 190) ){
                judge_msg.input_position_judge_success_count ++;
                if(judge_msg.input_position_judge_success_count >= 1){
                  judge_msg.input_position_judge = false;
                  judge_msg.calibrate_car_flag = false;
                  judge_msg.input_position_judge_success_count = 0;
                }
              }
              // else{
              //   car_move(car_msg.transright, 0.01);
              // }
              wk->clear_rec();
            }
            else{
              //Serial.println("nothing");
            }
            wk->clear_rec();
          }
      }

    }
  }
  car_pwm(car_msg.stop, 0, 0, 0, 0);
  Serial.println("judge success");
  // while(1){delay(100);}
}


///*
//通过色环处的圆来初步校准小车的位置
void calibrate_the_car_through_circle(){
  // servo_move(51, 139, 160, 25, 1500);delay(1000);
  // lift_move(45);delay(1000);
  // servo_move(115, 139, 160, 25, 1500);delay(1000);
  // servo_move(115, 80, 160, 25, 1500);delay(1000);

  judge_msg.input_circle_judge = true;
  judge_msg.circle_width = true;
  judge_msg.circle_height = false;

  while(judge_msg.input_circle_judge)
  {
    for(int i=0;i<1;i++)
    {
      judge_msg.send_commond_msg = "change_car_position";
      Serial2.println(judge_msg.send_commond_msg);
      //Serial.println(judge_msg.send_commond_msg);
      delay(200);

      //小车进入圆中心坐标的横坐标位置调整
      if(judge_msg.circle_width)
      {
        for(int j=0;j<3;j++)
        {
            wk->update_data(); //更新消息
            bool rt = wk->recive_box_msg(result , 2);//获取消息
            if(rt){
              judge_msg.center_x = result->x;
              judge_msg.center_y = result->y;
              Serial.print("x:");Serial.print(judge_msg.center_x);
              Serial.print(" y:");Serial.println(judge_msg.center_y);
              float move_distance_x = 0.0;
              float move_distance_y = 0.0;
              //横坐标需要移动的距离
              if(judge_msg.center_x < 140){
                move_distance_x = mapFloat(judge_msg.center_x,  0, 140, 0.10,  0);
                Serial.print("横坐标需向前移动:");Serial.println(move_distance_x, 4);
                car_move(car_msg.back, fabs(move_distance_x));
              }
              else if( (judge_msg.center_x >=140) && (judge_msg.center_x <=180) ){
                Serial.println("横坐标不需要移动");
              }
              else{
                move_distance_x = mapFloat(judge_msg.center_x,  180, 320, 0,  0.10);
                Serial.print("横坐标需要向后移动:");Serial.println(move_distance_x, 4);
                car_move(car_msg.forward, fabs(move_distance_x));
              }
              //纵坐标需要移动的距离
              if(judge_msg.center_y < 100){
                move_distance_y = mapFloat(judge_msg.center_y,  0, 100, 0.10,  0);
                Serial.print("纵坐标需要向左移动:");Serial.println(move_distance_y, 4);
                car_move(car_msg.transleft, fabs(move_distance_y));
              }
              else if( (judge_msg.center_y >=100) && (judge_msg.center_y <=140) ){
                Serial.println("纵坐标不需要移动");
                judge_msg.circle_width = false;
                judge_msg.input_circle_judge = false;
              }
              else{
                move_distance_y = mapFloat(judge_msg.center_y,  140, 240, 0,  0.10);
                Serial.print("纵坐标需要向右移动:");Serial.println(move_distance_y, 4);
                car_move(car_msg.transright, fabs(move_distance_y));
              }
              wk->clear_rec();
            }
            else{
              //Serial.println("nothing");
            }
            wk->clear_rec();
        }
      }
    }
  }
  Serial.println("break circle center loop");
  // while(1){delay(100);}
}
//*/

//-----------------------------------------------------------------------------------------------------------------------------------
void calibrate_the_car_through_circle2(){
  // servo_move(51, 139, 160, 25, 1500);delay(1000);
  // lift_move(45);delay(1000);
  // servo_move(115, 139, 160, 25, 1500);delay(1000);
  // servo_move(115, 80, 160, 25, 1500);delay(1000);

  judge_msg.input_circle_judge = true;
  judge_msg.circle_width = true;
  judge_msg.circle_height = false;

  while(judge_msg.input_circle_judge)
  {
    for(int i=0;i<1;i++)
    {
      judge_msg.send_commond_msg = "change_car_position";
      Serial2.println(judge_msg.send_commond_msg);
      //Serial.println(judge_msg.send_commond_msg);
      delay(200);

      //小车进入圆中心坐标的横坐标位置调整
      if(judge_msg.circle_width)
      {
        for(int j=0;j<3;j++)
        {
            wk->update_data(); //更新消息
            bool rt = wk->recive_box_msg(result , 2);//获取消息
            if(rt){
              judge_msg.center_x = result->x;
              judge_msg.center_y = result->y;
              Serial.print("x:");Serial.print(judge_msg.center_x);
              Serial.print(" y:");Serial.println(judge_msg.center_y);
              float move_distance_x = 0.0;
              float move_distance_y = 0.0;
              //横坐标需要移动的距离
              if(judge_msg.center_x < 140){
                move_distance_x = mapFloat(judge_msg.center_x,  0, 140, 0.10,  0);
                Serial.print("横坐标需向前移动:");Serial.println(move_distance_x, 4);
                // car_move(car_msg.back, fabs(move_distance_x));
                car_move(car_msg.transright, fabs(move_distance_y));
              }
              else if( (judge_msg.center_x >=140) && (judge_msg.center_x <=180) ){
                Serial.println("横坐标不需要移动");
              }
              else{
                move_distance_x = mapFloat(judge_msg.center_x,  180, 320, 0,  0.10);
                Serial.print("横坐标需要向后移动:");Serial.println(move_distance_x, 4);
                // car_move(car_msg.forward, fabs(move_distance_x));
                car_move(car_msg.transleft, fabs(move_distance_y));
              }
              //纵坐标需要移动的距离
              if(judge_msg.center_y < 100){
                move_distance_y = mapFloat(judge_msg.center_y,  0, 100, 0.10,  0);
                Serial.print("纵坐标需要向左移动:");Serial.println(move_distance_y, 4);
                // car_move(car_msg.transleft, fabs(move_distance_y));
                // car_move(car_msg.transright, fabs(move_distance_y));
                car_move(car_msg.back, fabs(move_distance_x));
              }
              else if( (judge_msg.center_y >=100) && (judge_msg.center_y <=140) ){
                Serial.println("纵坐标不需要移动");
                judge_msg.circle_width = false;
                judge_msg.input_circle_judge = false;
              }
              else{
                move_distance_y = mapFloat(judge_msg.center_y,  140, 240, 0,  0.10);
                Serial.print("纵坐标需要向右移动:");Serial.println(move_distance_y, 4);
                // car_move(car_msg.transright, fabs(move_distance_y));
                // car_move(car_msg.transleft, fabs(move_distance_y));
                car_move(car_msg.forward, fabs(move_distance_x));
              }
              wk->clear_rec();
            }
            else{
              //Serial.println("nothing");
            }
            wk->clear_rec();
        }
      }
    }
  }
  Serial.println("break circle center loop");
  // while(1){delay(100);}
}


//=======================================================================================
//=======================================================================================
//二维码检测部分代码~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=======================================================================================
//=======================================================================================
void qrcode_detection_loop(){
  while(qr_msg.qr_detection_loop){
    while( !qr_msg.recv_qr_msg_success ){
      //开始向视觉模块发送开始检测二维码命令
      if(qr_msg.recv_qr_msg_start == false){
        for(int i=0;i<1;i++){
          qr_msg.send_commond_msg = "qr_detect_start";
          Serial2.println(qr_msg.send_commond_msg);
          delay(200);
          for(int i=0;i<3;i++){
            qr_detect_start();
          }
        }      
      }
      //当接收到二维码后，检测视觉模块二维码反馈成功标志
      else{
        qr_msg.send_commond_msg = "qr_detect_success";
        Serial2.println(qr_msg.send_commond_msg);
        delay(200);
        for(int i=0;i<3;i++){
          qr_detect_success();
        }
      }
    }
    //Serial.println("break qr loop");
  }
  // Serial.print("x:");Serial.print(qr_msg.qr_x_pos);Serial.print(" | ");
  // Serial.print("y:");Serial.print(qr_msg.qr_y_pos);Serial.print(" | ");
  // Serial.print("w:");Serial.print(qr_msg.qr_w_pos);Serial.print(" | ");
  // Serial.print("h:");Serial.print(qr_msg.qr_h_pos);Serial.print(" | ");
  // Serial.print("msg:");Serial.println(qr_msg.qr_data);
}

//实时检测接收到视觉模块发送的信息
void qr_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_QRCODES);//获取消息
  if(rt)//若获取到新的消息
  {
    char str1[10] = "";
    String recv_data = "";
    strcpy(str1, result->msg);
    for(int i=0;i<sizeof(str1);i++){
      recv_data += str1[i];
    }
    int num = recv_data.compareTo("qr_success");
    if( num == 0  ){
      qr_msg.recv_qr_msg_success = true;
      qr_msg.qr_detection_loop = false;
    }
    wk->clear_rec();
  }
}

//检测上位机给下位机是否发送了二维码数据
void qr_detect_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_QRCODES);//获取消息
  if(rt)//若获取到新的消息
  {
    qr_msg.qr_x_pos = result->x;
    qr_msg.qr_y_pos = result->y;
    qr_msg.qr_w_pos = result->w;
    qr_msg.qr_w_pos = result->h;
    qr_msg.qr_data  = result->msg;
    qr_msg.recv_qr_msg_start = true;
    char str1[10] = "";
    strcpy(str1, result->msg);
    for(int i=0;i<sizeof(str1);i++){
      detect_msg.qr_complete_data += str1[i];
    }
    //处理二维码数据，将二维码对应的字符串数据转换为int类型数据
    process_qr_string_data(detect_msg.qr_complete_data);
    // detect_msg.qr_complete_data.trim();
    // int special_symbols = detect_msg.qr_complete_data.indexOf('+');
    // String data1 = detect_msg.qr_complete_data.substring(0, special_symbols);
    // String data2 = detect_msg.qr_complete_data.substring(special_symbols+1);
    // Serial.print("data1:");Serial.println(data1);
    // Serial.print("data2:");Serial.println(data2); 
    wk->clear_rec();
  }
  //wk->clear_rec();
}

//处理二维码数据，将二维码对应的字符串数据转换为int类型数据
void process_qr_string_data(String process_str){
  detect_msg.qr_complete_data.trim();
  int special_symbols = detect_msg.qr_complete_data.indexOf('+');
  String data1 = detect_msg.qr_complete_data.substring(0, special_symbols);
  String data2 = detect_msg.qr_complete_data.substring(special_symbols+1);
  // Serial.print("before_qr_str:");Serial.println(data1);
  // Serial.print("after_qr_str:");Serial.println(data2);
  detect_msg.before_the_plus_data = data1.toInt();
  detect_msg.after_the_plus_data = data2.toInt();
  // Serial.print("检测到的二维码信息为:");Serial.println(detect_msg.qr_complete_data);
  // Serial.print("二维码加号之前的数据:");Serial.println(detect_msg.before_the_plus_data);
  // Serial.print("二维码加号之后的数据:");Serial.println(detect_msg.after_the_plus_data);
}

void colorring_detection_loop(){
  colorring_msg.colorring_detection_loop = true;
  colorring_msg.recv_colorring_msg_success = false;
  colorring_msg.recv_colorring_msg_start = false;
  detect_msg.detection_colorring_sum_num ++;
  while(colorring_msg.colorring_detection_loop)
  {
    while(!colorring_msg.recv_colorring_msg_success)
    {
      if(colorring_msg.recv_colorring_msg_start == false)
      {
        colorring_msg.send_commond_msg = "color_ring_start";
        Serial2.println(colorring_msg.send_commond_msg);
        //Serial.println(colorring_msg.send_commond_msg);
        delay(200);
        colorring_detection_start();
      }
      else{
        colorring_msg.send_commond_msg = "color_ring_success";
        Serial2.println(colorring_msg.send_commond_msg);
        Serial.println("send_color_ring_success");
        delay(200);
        for(int i=0;i<3;i++){
          colorring_detect_success();
        }
      }
    }
  }
  // Serial.println("colorring break");
  wk->clear_rec();
  // delay(1000);
  switch(detect_msg.detection_colorring_sum_num){
    case 1: {detect_msg.recv_colorring_one   = detect_msg.receive_current_colorring;} break;
    case 2: {detect_msg.recv_colorring_two   = detect_msg.receive_current_colorring;} break;
    case 3: {detect_msg.recv_colorring_three = detect_msg.receive_current_colorring;} break;
    case 4: {detect_msg.recv_colorring_four  = detect_msg.receive_current_colorring;} break;
    case 5: {detect_msg.recv_colorring_five  = detect_msg.receive_current_colorring;} break;
    case 6: {detect_msg.recv_colorring_six   = detect_msg.receive_current_colorring;} break;
    default: break;
  }
}

void colorring_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_OBJECT);//获取消息
  if(rt)//若获取到新的消息
  {
    if( strcmp(result->msg, "colorring_success") == 0 ){
      colorring_msg.recv_colorring_msg_success = true;
      colorring_msg.colorring_detection_loop = false;
      wk->clear_rec();
    }
  }
}

void colorring_detection_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_OBJECT);//获取消息
  if(rt)//若获取到新的消息
  {
    colorring_msg.colorring_x_pos = result->x;
    colorring_msg.colorring_y_pos = result->y;
    colorring_msg.colorring_w_pos = result->w;
    colorring_msg.colorring_h_pos = result->h;
    colorring_msg.colorring_data  = result->msg;
    if(strcmp(result->msg, "red") == 0) {
      // Serial.print("I found red color ring. ");
      detect_msg.receive_current_colorring = detect_msg.RED_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    if(strcmp(result->msg, "green") == 0) {
      // Serial.print("I found green color ring. ");
      detect_msg.receive_current_colorring = detect_msg.GREEN_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    if(strcmp(result->msg, "blue") == 0) {
      // Serial.print("I found blue color ring. ");
      detect_msg.receive_current_colorring = detect_msg.BLUE_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    wk->clear_rec();
  }
}
//=======================================================================================
//=======================================================================================
//色块检测部分代码~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=======================================================================================
//=======================================================================================
//小车进入颜色检测循环，直到小车接收到颜色信息，才会退出该循环
void color_detection_loop(){
  color_msg.color_detection_loop = true;
  color_msg.recv_color_msg_success = false;
  color_msg.recv_color_msg_start = false;
  detect_msg.detection_color_sum_num ++;
  
  // while(detect_msg.catch_material_flag){
    while(color_msg.color_detection_loop){
      while(!color_msg.recv_color_msg_success){
        if(color_msg.recv_color_msg_start == false){
          color_msg.send_commond_msg = "color_detect_start";
          Serial2.println(color_msg.send_commond_msg);
          // Serial.println(color_msg.send_commond_msg);
          delay(200);
          for(int i=0;i<3;i++){
            color_detect_start();
          }
        }
        else{
          color_msg.send_commond_msg = "color_detect_success";
          Serial2.println(color_msg.send_commond_msg);
          // Serial.println(color_msg.send_commond_msg);
          delay(200);
          for(int i=0;i<3;i++){
            color_detect_success();
          }
        }
      }
    }

  // }
  // Serial.print("x:");Serial.print(color_msg.color_x_pos);Serial.print(" | ");
  // Serial.print("y:");Serial.print(color_msg.color_y_pos);Serial.print(" | ");
  // Serial.print("w:");Serial.print(color_msg.color_w_pos);Serial.print(" | ");
  // Serial.print("h:");Serial.print(color_msg.color_h_pos);Serial.print(" | ");
  // Serial.print("msg:");Serial.println(color_msg.color_data);
  wk->clear_rec();
  // delay(1000);
  // switch(detect_msg.detection_color_sum_num){
  //   case 1: {detect_msg.recv_color_one = detect_msg.recv_current_color;} break;
  //   case 2: {detect_msg.recv_color_two = detect_msg.recv_current_color;} break;
  //   case 3: {detect_msg.recv_color_three = detect_msg.recv_current_color;} break;
  //   default: break;
  // }
}

//检测上位机给下位机是否发送了颜色信息数据
void color_detect_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);//获取消息
  if(rt)//若获取到新的消息
  {
    color_msg.color_x_pos = result->x;
    color_msg.color_y_pos = result->y;
    color_msg.color_w_pos = result->w;
    color_msg.color_w_pos = result->h;
    color_msg.color_data  = result->msg;
    if(strcmp(result->msg, "Redsth") == 0) {
      // Serial.print("I found red. ");
      detect_msg.recv_current_color = detect_msg.RED_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    if(strcmp(result->msg, "Greensth") == 0) {
      // Serial.print("I found green. ");
      detect_msg.recv_current_color = detect_msg.GREEN_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    if(strcmp(result->msg, "Bluesth") == 0) {
      // Serial.print("I found blue. ");
      detect_msg.recv_current_color = detect_msg.BLUE_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    // color_msg.recv_color_msg_start = true;
    // Serial.print("x:");Serial.print(color_msg.color_x_pos);Serial.print(" | ");
    // Serial.print("y:");Serial.print(color_msg.color_y_pos);Serial.print(" | ");
    // Serial.print("w:");Serial.print(color_msg.color_w_pos);Serial.print(" | ");
    // Serial.print("h:");Serial.print(color_msg.color_h_pos);Serial.print(" | ");
    // Serial.print("msg:");Serial.println(color_msg.color_data);
    wk->clear_rec();
  }
}

//实时检测接收到视觉模块发送的信息
void color_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);//获取消息
  if(rt)//若获取到新的消息
  {
    if( strcmp(result->msg, "color_success") == 0 ){
      color_msg.recv_color_msg_success = true;
      color_msg.color_detection_loop = false;
      // detect_msg.catch_material_flag = false;
      wk->clear_rec();
    }
  }
}

//处理二维码数据.将二维码信息转换为抓取和放置物料顺序
void process_qr_msg(){
  detect_msg.qr_code_capture_sequence_three = detect_msg.before_the_plus_data % 10;
  detect_msg.qr_code_capture_sequence_two = (detect_msg.before_the_plus_data / 10) % 10;
  detect_msg.qr_code_capture_sequence_one = (detect_msg.before_the_plus_data / 100) % 10;

  detect_msg.qr_code_placement_order_three = detect_msg.after_the_plus_data % 10;
  detect_msg.qr_code_placement_order_two = (detect_msg.after_the_plus_data / 10) % 10;
  detect_msg.qr_code_placement_order_one = (detect_msg.after_the_plus_data /100) % 10;

  Serial.print("计算得到小车抓取物料颜色代号分别为: ");
  Serial.print(detect_msg.qr_code_capture_sequence_one);Serial.print(" ");
  Serial.print(detect_msg.qr_code_capture_sequence_two);Serial.print(" ");
  Serial.println(detect_msg.qr_code_capture_sequence_three);

  Serial.print("计算得到小车放置物料颜色代号分别为: ");
  Serial.print(detect_msg.qr_code_placement_order_one);Serial.print(" ");
  Serial.print(detect_msg.qr_code_placement_order_two);Serial.print(" ");
  Serial.println(detect_msg.qr_code_placement_order_three);
}

//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//之前的程序部分
void vision_test(){
  detect_qr();   //进入二维码检测，直到检测到二维码后，程序才退出
  color_detect();  //进入颜色检测，直到检测到三个颜色后，程序才退出
  colorring_detect(); //进入色环检测，直到检测到三个色环后，程序才退出
}

//进入二维码检测，直到检测到二维码后，程序才退出
void detect_qr(){
  qrcode_detection_loop();//小车进入二维码检测循环，直到小车接收到二维码，才会退出该循环
  Serial.print("检测到的二维码信息为:");Serial.println(detect_msg.qr_complete_data);
  Serial.print("二维码加号之前的数据:");Serial.println(detect_msg.before_the_plus_data);
  Serial.print("二维码加号之后的数据:");Serial.println(detect_msg.after_the_plus_data);
  delay(1000);
  //process_qr_msg();
}

//进入颜色检测，直到检测到三个颜色后，程序才退出
void color_detect(){
  color_detection_loop(); //第一次检测颜色
  Serial.print("第一次检测到的颜色为:");Serial.println(detect_msg.recv_color_one);
  delay(1000);
  color_detection_loop(); //第二次检测颜色
  Serial.print("第二次检测到的颜色为:");Serial.println(detect_msg.recv_color_two);
  delay(1000);
  color_detection_loop(); //第三次检测颜色
  Serial.print("第三次检测到的颜色为:");Serial.println(detect_msg.recv_color_three);
  delay(1000);
}

//进入色环检测，直到检测到三个色环后，程序才退出
void colorring_detect(){
  colorring_detection_loop(); //第一次检测色环
  Serial.print("第一次检测到的色环颜色为:");Serial.println(detect_msg.recv_colorring_one);
  delay(2000);
  colorring_detection_loop(); //第二次检测色环
  Serial.print("第二次检测到的色环颜色为:");Serial.println(detect_msg.recv_colorring_two);
  delay(2000);
  colorring_detection_loop(); //第三次检测色环
  Serial.print("第三次检测到的色环颜色为:");Serial.println(detect_msg.recv_colorring_three);
  delay(2000);
}

